///////////////////////////////////////////////////////////
//
// File: HaptWiimote.cpp
//
// Description:
//
///////////////////////////////////////////////////////////

// Local Includes
#include "HaptCtrl.h"      // Required PCH
#include "HaptWiimote.h"   // Required Win32
#include "HaptHIDDevice.h"

// System Includes
#include <assert.h>
#include <iostream>

static const unsigned int WIIMOTE_VENDOR_ID  = 0x057E;
static const unsigned int WIIMOTE_PRODUCT_ID = 0x0306;

#define s_PI 3.14159265

char *aWiiButtonName [11] = { "2",
                              "1",
                              "B",
                              "A",
                              "MIN",
                              "HOME",
                              "Left",
                              "Right",
                              "Down",
                              "Up",
                              "PLUS" };

unsigned int aWiiButtonCode[11] = { 0x0001,
                                    0x0002,
                                    0x0004,
                                    0x0008,
                                    0x0010,
                                    0x0080,
                                    0x0100,
                                    0x0200,
                                    0x0400,
                                    0x0800,
                                    0x1000  };

vector<HaptWiimote>   s_vWiimote;
vector<HaptHIDDevice> s_vDevice;

HaptWiimote::HaptWiimote()
:
  _abLeds(),
  _abButtonPressed(),
  _bRumble(false),
  _abIR(),
  _asIR(),
  _fMotion(),
  _fIR(),
  _uiRawMotion(),
  _uiRawIR(),
  _fNormIR(),
  _bCal(false),
  _uiMotionCal(),
  _uiGCal(),
  _mode(WII_OUTPUT_BUTMOTIR),
  _bCont(true),
  _bThread(false),
  _threadID(-1),
  _hThread(INVALID_HANDLE_VALUE),
  _pHIDDevice(NULL),
  _pCallback(NULL),
  _inBuf(),
  _outBuf(),
  _tmpBuf()
{
    _uiGCal[0] = 1.0f;
    _uiGCal[1] = 1.0f;
    _uiGCal[2] = 1.0f;
}

HaptWiimote::~HaptWiimote()
{
    if ( _hThread != INVALID_HANDLE_VALUE ) {
        _bThread = false;
        WaitForSingleObject( _hThread, INFINITE );
        // Destroy the thread object.
        //CloseHandle( _hThread ); //TOXIC: Causes crash
        _hThread = INVALID_HANDLE_VALUE;
    }
}

bool 
HaptWiimote::isButtonPressed( HaptWiimoteButton eBut )
{
    if ( eBut < 0 || eBut > 10 )
        return false;
    
    return _abButtonPressed[eBut];
}

bool 
HaptWiimote::getMotionData( float &x, float &y, float &z )
{
    x = _fMotion[0];
    y = _fMotion[1];
    z = _fMotion[2];

    return true;
}

bool 
HaptWiimote::getRawMotionData( unsigned int &x, unsigned int &y, unsigned int &z )
{
    x = _uiRawMotion[0];
    y = _uiRawMotion[1];
    z = _uiRawMotion[2];

    return true;
}

bool
HaptWiimote::getRawIRData( float &x1, float &y1, 
                           float &x2, float &y2,
                           float &x3, float &y3, 
                           float &x4, float &y4 )
{
    x1 = _uiRawIR[0][0] - 512.0f;
    y1 = _uiRawIR[0][1] - 384.0f;
    x2 = _uiRawIR[1][0] - 512.0f;
    y2 = _uiRawIR[1][1] - 384.0f;
    x3 = _uiRawIR[2][0] - 512.0f;
    y3 = _uiRawIR[2][1] - 384.0f;
    x4 = _uiRawIR[3][0] - 512.0f;
    y4 = _uiRawIR[3][1] - 384.0f;

    return true;
}

bool
HaptWiimote::getIRData( float &x1, float &y1, 
                      float &x2, float &y2,
                      float &x3, float &y3, 
                      float &x4, float &y4 )
{
    x1 = _fIR[0][0];
    y1 = _fIR[0][1];
    x2 = _fIR[1][0];
    y2 = _fIR[1][1];
    x3 = _fIR[2][0];
    y3 = _fIR[2][1];
    x4 = _fIR[3][0];
    y4 = _fIR[3][1];

    return true;
}

bool 
HaptWiimote::getPosition( float &x, float &y, float &z )
{
    // TOXIC: Need to rotate the IR based 
    //        upon the roll of the wiimote

    // x' = cos(theta)*x-sin(theta)*y
    // y' = sin(theta)*x+cos(theta)*y

    // LL,UL,LR,UR
    int p[4] = {0,1,2,3};
    _sort(p);

    float lenLeft  = _length(_uiRawIR[p[0]],_uiRawIR[p[1]]);
    float lenRight = _length(_uiRawIR[p[2]],_uiRawIR[p[3]]);
    float lenUpper = _length(_uiRawIR[p[1]],_uiRawIR[p[3]]);
    float lenLower = _length(_uiRawIR[p[0]],_uiRawIR[p[2]]);

    float distLeft  = _distance(lenLeft);
    float distRight = _distance(lenRight);
    float distUpper = _distance(lenUpper);
    float distLower = _distance(lenLower);

    // Calculate the the horizontal angle and x position
    float C = acos( (100 + distLeft*distLeft - distRight*distRight)/(2*10*distLeft) );
    x = 5-distLeft*cos(C);
    
    // Calculate the z position relative to x
    z = distLeft*sin(C);
    
    // TOXIC: Need to verify everything below.

    // Calculate the vertical angle and y position
    C = acos( (100 + distLower*distLower - distUpper*distUpper)/(2*10*distLower) );
    y = 5-distLower*cos(C);

    // Recalculate the z position using the vertical angle
    //z = z * sin( C );

    return true;
}

bool 
HaptWiimote::getNormIRData( float &x, float &y )
{
    x = _fNormIR[0];
    y = _fNormIR[1];

    return true;
}

float 
HaptWiimote::pitch() 
{ 
    return asin(_fMotion[1])*180.0f/s_PI; 
}
    
float 
HaptWiimote::roll()  
{ 
    return -asin(_fMotion[0])*180.0f/s_PI; 
}

float 
HaptWiimote::pitchRad() 
{ 
    return asin(_fMotion[1]); 
}
    
float 
HaptWiimote::rollRad()  
{ 
    return -asin(_fMotion[0]); 
}

bool 
HaptWiimote::setLEDs(bool led1, bool led2, bool led3, bool led4)
{
    DWORD outBytes;
	byte out[2] = { WII_INPUT_LEDS, _bRumble ? 0x01 : 0x00 };

	if (led1) out[1] |= 0x10;
	if (led2) out[1] |= 0x20;
	if (led3) out[1] |= 0x40;
	if (led4) out[1] |= 0x80;

    return HaptHIDDevice::outputToDevice(*_pHIDDevice,out,outBytes);
}

bool 
HaptWiimote::setRumble(bool on)
{
    bool res = true;
    DWORD outBytes;
    if( _bRumble != on ) {
        _bRumble = on;
	    byte out[2] = { WII_INPUT_STATUS, _bRumble ? 0x01 : 0x00 };  
        res = HaptHIDDevice::outputToDevice(*_pHIDDevice,out,outBytes);
    }

    return res;
}

bool 
HaptWiimote::setButton( int iBut, bool bEnable )
{
    if ( iBut < 0 || iBut > 10 )
        return false;
    
    _abButtonPressed[iBut] = bEnable;

    return true;
}

bool 
HaptWiimote::setDefaultState()
{
    return true;
}

bool 
HaptWiimote::setMode( byte mode )
{
    switch (mode) {
          _requestType(mode);
      case WII_OUTPUT_BUTMOTIR:
          _requestIR(0x03); // Extended
          _requestType(mode);
          break;
      case WII_OUTPUT_EXTMOTIR:
          _requestIR(0x01); // Basic
          _requestType(mode);
          break;
      default:
          _requestType(mode);
    };

    return true;
}

unsigned CALLBACK 
HaptWiimote::wiimoteThreadCB(void * param)
{
    DWORD outBytes;
    HaptWiimote * pDevice = reinterpret_cast<HaptWiimote*>(param);

    // End the thread if threading is set to false
    // on the HaptWiimote
    if ( pDevice->_bThread == false ) { _endthreadex(0); }

	while (pDevice->_bThread)
	{
		ZeroMemory(pDevice->_inBuf, 256);
        
        // TOXIC: Need to add error handling?
        DWORD result = pDevice->_pHIDDevice->queryEvent(pDevice->_inBuf,outBytes);
		
        // QUESTION: Why does this size come back as zero?
        pDevice->_handleEvents(pDevice->_inBuf);
	}

	return 0;
}

bool 
HaptWiimote::enableThread( bool bEnable )
{
    if ( bEnable ) {

	    // Attempt to create a thread
	    _hThread = (HANDLE)_beginthreadex(0,0,wiimoteThreadCB,this,0,&_threadID); 

        // Return false if we failed to create the thread
        if ( _hThread == INVALID_HANDLE_VALUE ) return false;

        // Set thread to true
        _bThread = true;

        _requestCalibration();

    } else {
        // TOXIC: How do we signal the thread to kill itself?
        _bThread = false;
    }

	return true;
}

bool HaptWiimote::_handleEvents( byte* pByte )
{
    HaptWiiEvent wii;

    switch (pByte[0]) {
        case WII_OUTPUT_STATUS:
            _decodeStatus(pByte);
            break;
        case WII_OUTPUT_READDATA:
            _decodeButtons(pByte);
            _decodeData(pByte);
            break;
        case WII_OUTPUT_BUTTON:
            _decodeButtons(pByte);
            break;
        case WII_OUTPUT_BUTMOT:
            _decodeButtons(pByte);
            _decodeMotion(pByte);
            break;
        case WII_OUTPUT_BUTMOTIR:
            _decodeButtons(pByte);
            _decodeMotion(pByte);
            _decodeIR(pByte);
            break;
        case WII_OUTPUT_EXTMOT:  
            _decodeButtons(pByte);
            _decodeMotion(pByte);
            _decodeExtension(pByte);
            break;
        case WII_OUTPUT_EXTMOTIR:
            _decodeButtons(pByte);
            _decodeMotion(pByte);
            _decodeIR(pByte);
            _decodeExtension(pByte);
            break;
        default:
            break;
    };

    // Forward to the callback
    if ( _pCallback ) (_pCallback)(this,wii);

    return true;
}

bool
HaptWiimote::_decodeData( byte *pByte )
{
    if ( pByte[3] & 0x08 || pByte[3] & 0x07 ) {
        // TOXIC: How should we handle these error conditions
    } else {
		int size = pByte[3] >> 4;
        memset(_tmpBuf,0x00,size);
        memcpy(_tmpBuf,pByte+6,size+1);
	}

    return true;
}

bool 
HaptWiimote::_decodeButtons( byte *pByte )
{
    // Read in the button data
    // Note: Stored bigedian
    int data = _swap2Bytes(pByte+1);
    for ( int x=0; x<11; x++ ) {
        setButton( x, data & aWiiButtonCode[x] );
    }

    return true;
}

bool 
HaptWiimote::_decodeMotion( byte *pByte )
{
    _uiRawMotion[0] = pByte[3];
    _uiRawMotion[1] = pByte[4]; 
    _uiRawMotion[2] = pByte[5];

    _fMotion[0] = ((float)_uiRawMotion[0] - (float)_uiMotionCal[0]) /
                  ((float)_uiGCal[0] - (float)_uiMotionCal[0]);
    _fMotion[1] = ((float)_uiRawMotion[1] - (float)_uiMotionCal[1]) /
                  ((float)_uiGCal[1] - (float)_uiMotionCal[1]);
    _fMotion[2] = ((float)_uiRawMotion[2] - (float)_uiMotionCal[2]) /
                  ((float)_uiGCal[2] - (float)_uiMotionCal[2]);

    return true;
}

#define rotX( x, cx, y, cy, ang ) cx+cos(ang)*(x-512.0f-cx)-sin(ang)*(y-384.0f-cy)
#define rotY( x, cx, y, cy, ang ) cy+sin(ang)*(x-512.0f-cx)+cos(ang)*(y-384.0f-cy);

bool 
HaptWiimote::_decodeIR( byte *pByte )
{
    if ( _mode == WII_OUTPUT_EXTMOTIR ) {

        // Decode X1 & Y1
        _uiRawIR[0][0] = pByte[6]  | ((pByte[8] >> 4) & 0x03) << 8;
        _uiRawIR[0][1] = pByte[7]  | ((pByte[8] >> 6) & 0x03) << 8;

        // Decode X3 & Y3
        _uiRawIR[2][0] = pByte[11]  | ((pByte[13] >> 4) & 0x03) << 8;
        _uiRawIR[2][1] = pByte[12]  | ((pByte[13] >> 6) & 0x03) << 8;

        // Decode X2 & Y2
		_uiRawIR[1][0] = pByte[9]  | ((pByte[8] >> 0) & 0x03) << 8;
		_uiRawIR[1][1] = pByte[10] | ((pByte[8] >> 2) & 0x03) << 8;

        // Decode X4 & Y4
        _uiRawIR[3][0] = pByte[14]  | ((pByte[13] >> 0) & 0x03) << 8;
		_uiRawIR[3][1] = pByte[15] | ((pByte[13] >> 2) & 0x03) << 8;

        // Size is always 0 
		_asIR[0] = 0x00;
		_asIR[1] = 0x00;
        _asIR[2] = 0x00;
        _asIR[3] = 0x00;

        // Decode if detected
		_abIR[0] = !(pByte[6] == 0xff && pByte[7] == 0xff);
		_abIR[1] = !(pByte[9] == 0xff && pByte[10] == 0xff);
        _abIR[2] = !(pByte[11] == 0xff && pByte[12] == 0xff);
		_abIR[3] = !(pByte[14] == 0xff && pByte[15] == 0xff); 
 
    } else if ( _mode == WII_OUTPUT_BUTMOTIR ) {
        
        // Decode X1 & Y1
        _uiRawIR[0][0] = pByte[6]  | ((pByte[8] >> 4) & 0x03) << 8;
        _uiRawIR[0][1] = pByte[7]  | ((pByte[8] >> 6) & 0x03) << 8;

        // Decode X2 & Y2
		_uiRawIR[1][0] = pByte[9]  | ((pByte[11] >> 4) & 0x03) << 8;
		_uiRawIR[1][1] = pByte[10] | ((pByte[11] >> 6) & 0x03) << 8;

        // Decode X3 & Y3
        _uiRawIR[2][0] = pByte[12]  | ((pByte[14] >> 4) & 0x03) << 8;
        _uiRawIR[2][1] = pByte[13]  | ((pByte[14] >> 6) & 0x03) << 8;

        // Decode X4 & Y4
        _uiRawIR[3][0] = pByte[15]  | ((pByte[17] >> 4) & 0x03) << 8;
        _uiRawIR[3][1] = pByte[16]  | ((pByte[17] >> 6) & 0x03) << 8;

        // Decode the size
		_asIR[0] = pByte[8] & 0x0f;
		_asIR[1] = pByte[11] & 0x0f;
        _asIR[2] = pByte[14] & 0x0f;
		_asIR[3] = pByte[17] & 0x0f;

        // Decode if detected
		_abIR[0] = !(pByte[6]  == 0xff && pByte[7]  == 0xff && pByte[8]  == 0xff);
		_abIR[1] = !(pByte[9]  == 0xff && pByte[10] == 0xff && pByte[11] == 0xff); 
        _abIR[2] = !(pByte[12] == 0xff && pByte[13] == 0xff && pByte[14] == 0xff);
        _abIR[3] = !(pByte[15] == 0xff && pByte[16] == 0xff && pByte[17] == 0xff);

    } else {
        // TOXIC: Hard error
        return false;
    }

    float cx  = (_uiRawIR[0][0]+_uiRawIR[1][0]+_uiRawIR[2][0]+_uiRawIR[3][0])/4.0f-512.0f;
    float cy  = (_uiRawIR[0][1]+_uiRawIR[1][1]+_uiRawIR[2][1]+_uiRawIR[3][1])/4.0f-384.0f;
    float ang = rollRad();

    // IR 1
    _fIR[0][0] = rotX(_uiRawIR[0][0],cx,_uiRawIR[0][1],cy,ang);
    _fIR[0][1] = rotY(_uiRawIR[0][0],cx,_uiRawIR[0][1],cy,ang);

    // IR 2
    _fIR[1][0] = rotX(_uiRawIR[1][0],cx,_uiRawIR[1][1],cy,ang);
    _fIR[1][1] = rotY(_uiRawIR[1][0],cx,_uiRawIR[1][1],cy,ang);

    // IR3
    _fIR[2][0] = rotX(_uiRawIR[2][0],cx,_uiRawIR[2][1],cy,ang);
    _fIR[2][1] = rotY(_uiRawIR[2][0],cx,_uiRawIR[2][1],cy,ang);

    // IR4
    _fIR[3][0] = rotX(_uiRawIR[3][0],cx,_uiRawIR[3][1],cy,ang);
    _fIR[3][1] = rotY(_uiRawIR[3][0],cx,_uiRawIR[3][1],cy,ang);

    _normalizeIR();
    
    return true;
}

bool 
HaptWiimote::_decodeExtension( byte *pByte )
{
    // TOXIC: NYI
    return true;
}

bool 
HaptWiimote::_decodeStatus( byte *pByte )
{
    // TOXIC: NYI
    return true;
}

bool 
HaptWiimote::_encodeAddress( int address, byte *pByte )
{
    pByte[1] = (byte)((address >> 24) & 0xff) | (_bRumble ? 0x01 : 0x00);
	pByte[2] = (byte)((address >> 16) & 0xff);
	pByte[3] = (byte)((address >>  8) & 0xff);
	pByte[4] = (byte)(address & 0xff);

    return true;
}

bool 
HaptWiimote::_writeData( int address, byte *pBuff, int size )
{
    DWORD outBytes;
    memset(_outBuf,0x00,256);

    _outBuf[0] = WII_INPUT_WRITEMEM;
    _encodeAddress(address,_outBuf);
    _outBuf[5] = size;
    
    memcpy( _outBuf+6, pBuff, size );

    HaptHIDDevice::outputToDevice( *_pHIDDevice, pBuff, outBytes );
 
    SleepEx( 100, true );

    return true;
}

bool 
HaptWiimote::_readData(  int address, int size, byte *pBuff )
{
    DWORD outBytes;
    memset(_outBuf,0x00,256);

    _outBuf[0] = WII_INPUT_READMEM;
    _encodeAddress(address,_outBuf);
    _outBuf[5] = (byte)((size >> 8) & 0xff) ; // TOXIC:
	_outBuf[6] = (byte)(size & 0xff);

    HaptHIDDevice::outputToDevice( *_pHIDDevice, _outBuf, outBytes );

    SleepEx( 250, false );

    return true;
}

void 
HaptWiimote::_requestType( byte type )
{
    DWORD outBytes;
    memset(_outBuf,0x00,256);

    _outBuf[0] = WII_INPUT_TYPE;
    _outBuf[1] = _bCont ? 0x04 : 0x00 | _bRumble;  // TOXIC: What about continuous?
    _outBuf[2] = type;

    HaptHIDDevice::outputToDevice(*_pHIDDevice,_outBuf,outBytes);
}

void
HaptWiimote::_requestIR( byte type )
{
    DWORD outBytes;
    static byte a[] = {0x08};
    static byte b[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x00, 0xc0};
    static byte c[] = {0x40, 0x00};

    memset(_outBuf,0x00,256);
    _outBuf[0] = WII_INPUT_IR1;
    _outBuf[1] = (0x04) | (_bRumble ? 0x01 : 0x00);
    HaptHIDDevice::outputToDevice(*_pHIDDevice,_outBuf,outBytes);

    memset(_outBuf,0x00,256);
    _outBuf[0] = WII_INPUT_IR2;
    _outBuf[1] = (0x04) | (_bRumble ? 0x01 : 0x00);
    HaptHIDDevice::outputToDevice(*_pHIDDevice,_outBuf,outBytes);

    _writeData(0x04b00030,a,1);
    _writeData(0x04b00000,b,9);
    _writeData(0x04b0001a,c,2);
    _writeData(0x04b00033,&type,1);
}

void 
HaptWiimote::_requestCalibration()
{
    memset(_inBuf,0x00,256);
    _readData( 0x0016,7,_inBuf);

    _uiMotionCal[0] = _tmpBuf[0];
    _uiMotionCal[1] = _tmpBuf[1];
    _uiMotionCal[2] = _tmpBuf[2];
    _uiGCal[0] = _tmpBuf[4];
    _uiGCal[1] = _tmpBuf[5];
    _uiGCal[2] = _tmpBuf[6];
}

bool 
HaptWiimote::_normalizeIR()
{
    // Only provide a new normalize calculation if
    // all 4 IRs are currently being detected!
    if ( !_abIR[0] || !_abIR[1] || !_abIR[2] || !_abIR[3])
        return true;

    _fNormIR[0] = 0.0f;
    _fNormIR[1] = 0.0f;
    
    _fNormIR[0] = _uiRawIR[0][0] + _uiRawIR[1][0] + 
                  _uiRawIR[2][0] + _uiRawIR[3][0];
    _fNormIR[1] = _uiRawIR[0][1] + _uiRawIR[1][1] + 
                  _uiRawIR[2][1] + _uiRawIR[3][1];

    _fNormIR[0] = _fNormIR[0]/(4.0f*1024.0f); 
    _fNormIR[1] = _fNormIR[1]/(4.0f*768.0f);

    return true;
}

int 
HaptWiimote::_swap2Bytes( const unsigned char* p )
{
    // Perform bytes swap on two bytes
    int ret;
    ret = p[0];
	ret <<= 8;
	ret |= p[1];
	return ret;
}

int 
HaptWiimote::_swap4Bytes( const unsigned char* p )
{
    // TOXIC: Is this right?
    // Perform byte swap on four bytes
    int ret = 0;
	ret = p[0];
	ret <<= 24;
	ret |= p[1];
	ret <<= 16;
	ret |= p[2];
	ret <<= 8;
	ret |= p[3];
	return ret;
}

//////////////////////////////
// Static Interfaces
//////////

bool 
HaptWiimote::connect() 
{
    // Connect to all devices that are identified as
    // a Wiimote
    HaptHIDDevice::connectToDevices( WIIMOTE_VENDOR_ID, 
                                   WIIMOTE_PRODUCT_ID,
                                   s_vDevice );
    
    s_vWiimote.resize( s_vDevice.size() );
    size_t i, size = s_vWiimote.size();
    for ( i=0; i<size; i++ ) {
        s_vWiimote[i].setDevice(&(s_vDevice[i]));
    }

    return true;                              
}

bool 
HaptWiimote::disconnect()
{
    // Ensure the number of devices are equal to
    // the number of Wiimotes
    if ( s_vDevice.size() != s_vWiimote.size() )
        return false;

    size_t i, size = s_vWiimote.size();
    for ( i=0; i<size; i++ ) {
        // Disable all threaded Wiimote
	    if ( s_vWiimote[i].isThreaded() ) 
            s_vWiimote[i].enableThread(false);
        
        // Close any valid handles
        if ( s_vWiimote[i]._hThread != INVALID_HANDLE_VALUE ) {
            // Close the listening thread
            CloseHandle(s_vWiimote[i]._hThread);
        }

        // Close the corresponding device
         s_vDevice[i].disconnect();
    }

    // Set the lengths to zero
    s_vWiimote.clear();
    s_vDevice.clear();
    
    return true;
}

int        
HaptWiimote::numWiimotes()
{
    return (int)s_vWiimote.size();
}

HaptWiimote* 
HaptWiimote::wiimote( int idx )
{
    // Ensure idx is a valid value
    if ( idx < 0 || idx > s_vWiimote.size()-1 )
        return NULL;

    return &s_vWiimote[idx];
}